#导入MIC_ARRAY和LCD模块
from Maix import MIC_ARRAY as mic
import lcd
import time
import math
import sensor

#初始化模块
lcd.init()
mic.init()

sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QVGA)
sensor.set_hmirror(1)
sensor.set_vflip(1)

sensor.run(1)

while True:
# def get_mic_dir():
    AngleX=0
    AngleY=0
    AngleAddPi=0
    Angle=0
    AngleR=0
    FB=0
    # mic_list=[]
    imga = mic.get_map()
    b = mic.get_dir(imga)

    for i in range(len(b)):
        if b[i]>=2:
            AngleX+= b[i] * math.sin(i * math.pi/6)
            AngleY+= b[i] * math.cos(i * math.pi/6)
    AngleX=round(AngleX,6) #计算坐标转换值
    AngleY=round(AngleY,6)
    if AngleY<0:AngleAddPi=180
    if AngleX<0 and AngleY > 0:AngleAddPi=360
    if AngleX!=0 or AngleY!=0: #参数修正
        if AngleY==0:
            Angle=90 if AngleX>0 else 270 #填补X轴角度
        else:
            Angle=AngleAddPi+round(math.degrees(math.atan(AngleX/AngleY)),4) #计算角度
        AngleR=round(math.sqrt(AngleY*AngleY+AngleX*AngleX),4) #计算强度
        # mic_list.append(AngleX)
        # mic_list.append(AngleY)
        # mic_list.append(AngleR)
        # mic_list.append(Angle)
    a = mic.set_led(b,(0,0,255))# 配置 RGB LED 颜色值

    # z=math.sqrt(AngleX^2+AngleY^2)
    # q=int(z)
    lcd.draw_string(160, 0,"X:"+ str(AngleX), lcd.RED, lcd.BLACK)
    lcd.draw_string(160, 20, "Y:"+str(AngleY), lcd.RED, lcd.BLACK)
    lcd.draw_string(160, 40, "FB:"+str(AngleR), lcd.RED, lcd.BLACK)
    lcd.draw_string(160, 60, "Angle:"+str(Angle), lcd.RED, lcd.BLACK)
    # lcd.draw_string(160, 80, "z:"+str(z), lcd.RED, lcd.BLACK)
    #lcd.draw_string(100, 180, a, lcd.RED, lcd.BLACK)

    # return mic_list #返回列表，X坐标，Y坐标，强度，角度

    # 获取原始的声源黑白位图，尺寸 16*16
    imga = mic.get_map()

    # 获取声源方向并设置LED显示
    b = mic.get_dir(imga)
    a = mic.set_led(b, (0, 0, 255))

    # 将声源地图重置成正方形，彩虹色
    imgb = imga.resize(160, 160)

    img = sensor.snapshot()
    img1=img.resize(160, 160)
    lcd.display(img)

    imgc = imgb.to_rainbow(1)

    # 显示声源图
    lcd.display(imgc,oft=(0,0))
    # mic_list = get_mic_dir(b)
    # print(mic_list)

    time.sleep_ms(100)
# 关闭MIC_ARRAY模块
mic.deinit()